//左电机
int Lpwm =  9;
int Lin1 = 7;
int Lin2 = 8;

//右电机
int Rpwm = 11;
int Rin1 = 12;
int Rin2 = 13;


int isgun =0;
int isup = 0;
int isdown = 0;
//下面是四个42步进电机
//种植升降
unsigned int step3 = 10;
unsigned int dir3 = 6;

//step1,收获板子升降，En电机上上直接不连就行，反正是低电平有效
unsigned int step1 = 4;
unsigned int dir1 = 5;

//滚筒，只接step
unsigned int stepguntong = 3;

//种植上下动滚筒
unsigned int step4 = A0;
unsigned int dir4 = A1;


void maichong1(int times,int speed)//times是脉冲的数量，speed是脉冲间隔，对应着电机的速度
{
  for(int x=0; x<times; x++)
  {
    digitalWrite(step1,HIGH);
    delayMicroseconds(10);//这个函数单位为微秒
    digitalWrite(step1,LOW);
    delayMicroseconds(10);//驱动器说明书规定了脉冲信号的持续时间，在规定的时间内选择尽量小的数值
    delay(speed);//前后两个脉冲之间的间隔，毫秒
    //delayMicroseconds(1000);
  }
}
void maichong3(int times,int speed)//times是脉冲的数量，speed是脉冲间隔，对应着电机的速度
{
  for(int x=0; x<times; x++)
  {
    digitalWrite(step3,HIGH);
    delayMicroseconds(10);//这个函数单位为微秒
    digitalWrite(step3,LOW);
    delayMicroseconds(10);//驱动器说明书规定了脉冲信号的持续时间，在规定的时间内选择尽量小的数值
    delay(speed);//前后两个脉冲之间的间隔，毫秒
    //delayMicroseconds(1000);
  }
}

void maichong4(int times,int speed)//times是脉冲的数量，speed是脉冲间隔，对应着电机的速度
{
  for(int x=0; x<times; x++)
  {
    digitalWrite(stepguntong,HIGH);
    delayMicroseconds(10);//这个函数单位为微秒
    digitalWrite(stepguntong,LOW);
    delayMicroseconds(10);//驱动器说明书规定了脉冲信号的持续时间，在规定的时间内选择尽量小的数值
    delay(speed);//前后两个脉冲之间的间隔，毫秒
    //delayMicroseconds(1000);
  }
}
void maichong5(int times,int speed)//times是脉冲的数量，speed是脉冲间隔，对应着电机的速度
{
  for(int x=0; x<times; x++)
  {
    digitalWrite(step4,HIGH);
    delayMicroseconds(10);//这个函数单位为微秒
    digitalWrite(step4,LOW);
    delayMicroseconds(10);//驱动器说明书规定了脉冲信号的持续时间，在规定的时间内选择尽量小的数值
    delay(speed);//前后两个脉冲之间的间隔，毫秒
    //delayMicroseconds(1000);
  }
}

//step3,点那两个按钮就行了
void step1u_callback()
{
  digitalWrite(dir3,LOW);
  digitalWrite(LED_BUILTIN  , LOW);
  delayMicroseconds(10);
  unsigned long start_time1=millis();
  while(1)
  {
    maichong3(1000,1);
    unsigned long end_time1=millis();
    if(end_time1 - start_time1>=25000){
      break;
    }
  }
  
}
void step1d_callback()//当我按下down的按钮时，竟然是先调用btn_callback，然后再调用我这个函数，这太离谱了吧
//我现在把所有的命名全部改了一遍，发现这个问题就消失了！！！！难道是命名问题，之前组件的key是btn和btndown，改成了step1u和step1d
{
  digitalWrite(dir3,HIGH);
  digitalWrite(LED_BUILTIN  , HIGH);
  delayMicroseconds(10);
  unsigned long start_time2=millis();
  while(1)
  {
    maichong3(1000,1);
    unsigned long end_time2=millis();
    if(end_time2 - start_time2>=25000){
      break;
    }
  }
}
//step1
void btns_callback()
{
  digitalWrite(dir1,LOW);
  
  delayMicroseconds(10);
  unsigned long start_time1=millis();
  while(1)
  {
    maichong1(1000,1);
    unsigned long end_time1=millis();
    if(end_time1 - start_time1>=25000){
      break;
    }
  }
  
}
void btnj_callback()
{
  digitalWrite(dir1,HIGH);

  delayMicroseconds(10);
  unsigned long start_time2=millis();
  while(1)
  {
    maichong1(1000,1);
    unsigned long end_time2=millis();
    if(end_time2 - start_time2>=25000){
      break;
    }
  }
}



int speed = 70;

void motor(int speed,int dir,char num)//speed为转动速度(0-255)，dir为转动方向（0为正，1为反），num为不同电机（l为左，r为右）
{
  if(num == 'l')
  {
    analogWrite(Lpwm,speed);
    if(dir == 0)
    {
      digitalWrite(Lin1,HIGH);
      digitalWrite(Lin2,LOW);
    }
    else
    {
      digitalWrite(Lin1,LOW);
      digitalWrite(Lin2,HIGH);
    }
  }

  if(num == 'r')
  {
    analogWrite(Rpwm,speed);
    if(dir == 0)
    {
      digitalWrite(Rin1,HIGH);
      digitalWrite(Rin2,LOW);
    }
    else
    {
      digitalWrite(Rin1,LOW);
      digitalWrite(Rin2,HIGH);
    }
  }
}

void move(uint8_t xAxis,uint8_t yAxis)
{
  int x = xAxis;
  int y = yAxis;

  x = x-128;
  y = y-128;

  y = -y;
  if(x==0 && y==0){
    motor(0,0,'l');
    motor(0,0,'r');
    return;
  }
  if(x>=0)
  {
    if(y>=0){
      
      motor(speed,0,'l');
      
      motor(int(y*speed/128),0,'r');
      
    }
    else{
      motor(speed,1,'l');
      y = y*(-1);

      motor(int(y*speed/128),0,'r');
    }
  }
  else
  {
    x = x*(-1);

    if(y>=0){
      
      motor(int(x*speed/128),0,'l');
      
      motor(speed,0,'r');
    }
    else{
      
      motor(int(x*speed/128),0,'l');
      
      motor(speed,1,'r');
    }
  }


}

 
void setup() 
{
   
  Serial.begin(9600); // 初始化串口
  Serial.println("START");

  pinMode(Lpwm, OUTPUT);
  pinMode(Lin1, OUTPUT);
  pinMode(Lin2,OUTPUT);

  pinMode(Rpwm,OUTPUT);
  pinMode(Rin1,OUTPUT);
  pinMode(Rin2,OUTPUT);

  pinMode(step3, OUTPUT);//设置io口输出
  pinMode(dir3, OUTPUT);//设置io口输出
  pinMode(step1, OUTPUT);//设置io口输出
  pinMode(dir1, OUTPUT);//设置io口输出
  pinMode(step4, OUTPUT);//设置io口输出
  pinMode(dir4, OUTPUT);//设置io口输出
  pinMode(stepguntong, OUTPUT);//设置io口输出
  
  // motor(100,1,'l');
  // motor(100,1,'r');
}
 
 
void loop()
{
  if(isgun){
    maichong4(1000,1);
  }
  if(isup){
    digitalWrite(dir4,LOW);
    maichong5(1000,1);
  }

  if(isdown){
    digitalWrite(dir4,HIGH);
    maichong5(1000,1);
  } 
  
  if (Serial.available() > 0) {
    // 读取一行数据
    String received = Serial.readStringUntil('\n');
    
    // 判断是否以 "move" 开头
    if (received.startsWith("move")) {
      // 提取括号内的内容
      Serial.println("move");
      int start = received.indexOf('(') + 1;
      int end = received.indexOf(')');
      String data = received.substring(start, end);
      
      // 将数据分割为两个部分
      int commaIndex = data.indexOf(',');
      String x_str = data.substring(0, commaIndex);
      String y_str = data.substring(commaIndex + 1);
      
      // 将字符串转换为整数
      int x = x_str.toInt();
      int y = y_str.toInt();

      move(x,y);
      
      //打印接收到的数据
      Serial.print("接收到的数据：");
      Serial.print("x = ");
      Serial.print(x);
      Serial.print(", y = ");
      Serial.println(y);

    }
    else if(received.startsWith("qj")){
      Serial.println("qj");
      motor(speed,1,'l');
      motor(speed,1,'r');
      
    }
    else if(received.startsWith("ht")){
      Serial.println("ht");
      motor(speed,0,'l');
      motor(speed,0,'r');
      
    }
    else if(received.startsWith("stop")){
      Serial.println("stop");
      move(128,128);
    }
    else if(received.startsWith("up1")){
      Serial.println("up1");
      step1u_callback();
    }
    else if(received.startsWith("down1")){
      Serial.println("down1");
      step1d_callback();
    }
    else if(received.startsWith("up2")){
      Serial.println("up2");
      btns_callback();
    }
    else if(received.startsWith("down2")){
      Serial.println("down2");
      btnj_callback();
    }
    else if(received.startsWith("open3")){
      Serial.println("open3");
      isgun = 1;
    }
    else if(received.startsWith("close3")){
      Serial.println("close3");
      isgun = 0;
    }
    else if(received.startsWith("up4begin")){
      Serial.println("up4begin");
      isup = 1;
    }
    else if(received.startsWith("up4end")){
      Serial.println("up4end");
      isup = 0;
    }
    else if(received.startsWith("down4begin")){
      Serial.println("down4begin");
      isdown = 1;
    }
    else if(received.startsWith("down4end")){
      Serial.println("down4end");
      isdown = 0;
    }
    
    
  }
 
}
